#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control/action/move_action.hpp"
#include "std_msgs/msg/string.hpp"

#include <chrono>
#include <memory>
#include <thread>
#include <cmath>

using namespace std::chrono_literals;
using MoveAction = robot_control::action::MoveAction;
using GoalHandleMoveAction = rclcpp_action::ServerGoalHandle<MoveAction>;

class MoveActionServer : public rclcpp::Node
{
public:
    MoveActionServer() : Node("move_action_server"), current_x_(0.0), current_y_(0.0)
    {
        // 声明参数
        this->declare_parameter("max_linear_speed", 0.2);
        
        // 创建动作服务器
        action_server_ = rclcpp_action::create_server<MoveAction>(
            this,
            "move_to_goal",
            std::bind(&MoveActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MoveActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&MoveActionServer::handle_accepted, this, std::placeholders::_1));
        
        // 订阅障碍物警报
        obstacle_subscription_ = this->create_subscription<std_msgs::msg::String>(
            "/obstacle_alert", 10,
            std::bind(&MoveActionServer::obstacle_callback, this, std::placeholders::_1));
            
        RCLCPP_INFO(this->get_logger(), "移动动作服务器已启动");
    }

private:
    rclcpp_action::Server<MoveAction>::SharedPtr action_server_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr obstacle_subscription_;
    double current_x_, current_y_;
    bool has_obstacle_ = false;
    
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &uuid,
        std::shared_ptr<const MoveAction::Goal> goal)
    {
        (void)uuid;
        
        // 检查目标是否有效
        if (std::isnan(goal->target_x) || std::isnan(goal->target_y)) {
            RCLCPP_ERROR(this->get_logger(), "目标位置包含无效数值");
            return rclcpp_action::GoalResponse::REJECT;
        }
        
        RCLCPP_INFO(this->get_logger(), "收到移动目标: (%.2f, %.2f)", 
                   goal->target_x, goal->target_y);
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    
    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleMoveAction> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "移动任务被取消");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }
    
    void handle_accepted(const std::shared_ptr<GoalHandleMoveAction> goal_handle)
    {
        // 在新线程中执行动作，避免阻塞
        std::thread{std::bind(&MoveActionServer::execute, this, std::placeholders::_1), goal_handle}.detach();
    }
    
    void execute(const std::shared_ptr<GoalHandleMoveAction> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "开始执行移动任务");
        
        const auto goal = goal_handle->get_goal();
        auto result = std::make_shared<MoveAction::Result>();
        auto feedback = std::make_shared<MoveAction::Feedback>();
        
        double target_x = goal->target_x;
        double target_y = goal->target_y;
        double max_speed = this->get_parameter("max_linear_speed").as_double();
        
        bool success = true;
        
        // 计算总距离
        double total_distance = std::sqrt(
            std::pow(target_x - current_x_, 2) + 
            std::pow(target_y - current_y_, 2)
        );
        (void)total_distance;
        
        rclcpp::Rate loop_rate(2);  // 2Hz，每0.5秒更新一次
        
        while (rclcpp::ok() && success) {
            // 检查是否取消
            if (goal_handle->is_canceling()) {
                result->success = false;
                result->final_x = current_x_;
                result->final_y = current_y_;
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "移动任务已取消");
                return;
            }
            
            // 检查障碍物
            if (has_obstacle_) {
                RCLCPP_WARN(this->get_logger(), "检测到障碍物，停止移动");
                result->success = false;
                result->final_x = current_x_;
                result->final_y = current_y_;
                goal_handle->abort(result);
                return;
            }
            
            // 计算移动步长
            double dx = target_x - current_x_;
            double dy = target_y - current_y_;
            double distance = std::sqrt(dx * dx + dy * dy);
            
            // 检查是否到达目标
            if (distance < 0.05) {  // 5cm容差
                break;
            }
            
            // 计算移动方向
            double direction_x = dx / distance;
            double direction_y = dy / distance;
            
            // 计算这一步移动的距离（不超过最大速度）
            double step_distance = std::min(max_speed * 0.5, distance);  // 0.5秒的移动量
            
            // 更新当前位置
            current_x_ += direction_x * step_distance;
            current_y_ += direction_y * step_distance;
            
            // 更新反馈
            feedback->current_x = current_x_;
            feedback->current_y = current_y_;
            feedback->remaining_distance = distance - step_distance;
            
            goal_handle->publish_feedback(feedback);
            
            RCLCPP_DEBUG(this->get_logger(), "当前位置: (%.2f, %.2f), 剩余距离: %.2f", 
                        current_x_, current_y_, feedback->remaining_distance);
            
            loop_rate.sleep();
        }
        
        // 设置最终结果
        if (success) {
            result->success = true;
            result->final_x = current_x_;
            result->final_y = current_y_;
            goal_handle->succeed(result);
            RCLCPP_INFO(this->get_logger(), "移动任务完成，到达目标位置");
        }
    }
    
    void obstacle_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        // 检查消息是否包含障碍物警告
        if (msg->data.find("警告") != std::string::npos) {
            has_obstacle_ = true;
        } else {
            has_obstacle_ = false;
        }
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<MoveActionServer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}